//------------------------------------------------------------------------------
//  <copyright file="WorkingRegisterMessageManaged.cpp" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
//  </copyright>
//------------------------------------------------------------------------------
#include "Stdafx.h"
#include "KukaLBR4Definitions.h"
#include "WorkingRegisterMessageManaged.h"
#include "KukaLBR4Definitions.cs"


using namespace Microsoft::Robotics::Hardware::KukaLBR;

/// <summary> Initializes a new instance of the <c>WorkingRegisterMessageManaged</c> class. </summary>
WorkingRegisterMessageManaged::WorkingRegisterMessageManaged()
{
    this->Initialize();
}

/// <summary>Copy from managed array to unmanaged array. Assumed to be same length </summary>
template<class T>
bool WorkingRegisterMessageManaged::CopyFromManagedArray(array<T>^ from, size_t byteCount, T *to)
{
    int  minLength = Math::Min(from->Length, (int)(byteCount/sizeof(*to)));

    for (int i = 0 ; i < minLength ; ++i)
    {
        to[i] = from[i];
    }
    return true;
}

/// <summary> Copy to an unmanaged working register message </summary>
void WorkingRegisterMessageManaged::CopyToUnmanaged(KukaWorkingRegisterMessage *pMessage)
{
    pMessage->ControllerStrategyCommandRegister = this->ControllerStrategyCommandRegister;
	this->CurrentControllerStateNumber = pMessage->CurrentControllerStateNumber;
    pMessage->Scalar1Register = this->Scalar1Register;
    pMessage->Scalar2Register = this->Scalar2Register;
    pMessage->Scalar3Register = this->Scalar3Register;
    pMessage->Scalar4Register = this->Scalar4Register;
    pMessage->Scalar5Register = this->Scalar5Register;
    pMessage->Scalar6Register = this->Scalar6Register;
    pMessage->Scalar7Register = this->Scalar7Register;
    pMessage->Scalar8Register = this->Scalar8Register;

    this->CopyFromManagedArray<double>(this->JointPositionCommandRegister, sizeof(pMessage->JointPositionCommandRegister), pMessage->JointPositionCommandRegister);
    this->CopyFromManagedArray<double>(this->CartesianPositionCommandRegister, sizeof(pMessage->CartesianPositionCommandRegister), pMessage->CartesianPositionCommandRegister);
    this->CopyFromManagedArray<double>(this->JointTorqueCommandRegister, sizeof(pMessage->JointTorqueCommandRegister), pMessage->JointTorqueCommandRegister);
    this->CopyFromManagedArray<double>(this->TCPForceTorqueCommandRegister, sizeof(pMessage->TCPForceTorqueCommandRegister), pMessage->TCPForceTorqueCommandRegister);
    this->CopyFromManagedArray<double>(this->JointStiffnessCommandRegister, sizeof(pMessage->JointStiffnessCommandRegister), pMessage->JointStiffnessCommandRegister);
    this->CopyFromManagedArray<double>(this->JointDampingCommandRegister, sizeof(pMessage->JointDampingCommandRegister), pMessage->JointDampingCommandRegister);
    this->CopyFromManagedArray<double>(this->CartesianStiffnessCommandRegister, sizeof(pMessage->CartesianStiffnessCommandRegister), pMessage->CartesianStiffnessCommandRegister);
    this->CopyFromManagedArray<double>(this->CartesianDampingCommandRegister, sizeof(pMessage->CartesianDampingCommandRegister), pMessage->CartesianDampingCommandRegister);
    this->CopyFromManagedArray<double>(this->CartVec1Register, sizeof(pMessage->CartVec1Register), pMessage->CartVec1Register);
    this->CopyFromManagedArray<double>(this->CartVec2Register, sizeof(pMessage->CartVec2Register), pMessage->CartVec2Register);
    this->CopyFromManagedArray<double>(this->CartVec3Register, sizeof(pMessage->CartVec3Register), pMessage->CartVec3Register);
    this->CopyFromManagedArray<double>(this->JointVec1Register, sizeof(pMessage->JointVec1Register), pMessage->JointVec1Register);
    this->CopyFromManagedArray<double>(this->JointVec2Register, sizeof(pMessage->JointVec2Register), pMessage->JointVec2Register);
    this->CopyFromManagedArray<double>(this->JointVec3Register, sizeof(pMessage->JointVec3Register), pMessage->JointVec3Register);
	this->CopyFromManagedArray<double>(this->Frame1Register, sizeof(pMessage->Frame1Register), pMessage->Frame1Register);
	this->CopyFromManagedArray<double>(this->Frame2Register, sizeof(pMessage->Frame2Register), pMessage->Frame2Register);
	this->CopyFromManagedArray<double>(this->Frame3Register, sizeof(pMessage->Frame3Register), pMessage->Frame3Register);
}

/// <summary>
/// Copy from an array of arrays containing register values.
/// Registers must be in order given in <c>KukaLBR4PlusCsharpDefinitionsNativeMethods.KukaLBR4PlusWorkingRegisters</c>
/// </summary>
void WorkingRegisterMessageManaged::CopyFromArrayOfArrays(array<array<double>^>^ registers)
{
	this->ControllerStrategyCommandRegister = registers[KukaLBR4PlusWorkingRegisters::ControllerStrategyCommandRegister][0];
	this->CurrentControllerStateNumber = registers[KukaLBR4PlusWorkingRegisters::CurrentControllerStateNumber][0];
	this->Scalar1Register = registers[KukaLBR4PlusWorkingRegisters::Scalar1Register][0];
    this->Scalar2Register = registers[KukaLBR4PlusWorkingRegisters::Scalar2Register][0];
    this->Scalar3Register = registers[KukaLBR4PlusWorkingRegisters::Scalar3Register][0];
    this->Scalar4Register = registers[KukaLBR4PlusWorkingRegisters::Scalar4Register][0];
    this->Scalar5Register = registers[KukaLBR4PlusWorkingRegisters::Scalar5Register][0];
    this->Scalar6Register = registers[KukaLBR4PlusWorkingRegisters::Scalar6Register][0];
    this->Scalar7Register = registers[KukaLBR4PlusWorkingRegisters::Scalar7Register][0];
    this->Scalar8Register = registers[KukaLBR4PlusWorkingRegisters::Scalar8Register][0];

	Array::Copy(registers[KukaLBR4PlusWorkingRegisters::JointPositionCommandRegister], 0, this->JointPositionCommandRegister, 0, this->JointPositionCommandRegister->Length);
	Array::Copy(registers[KukaLBR4PlusWorkingRegisters::CartesianPositionCommandRegister], 0, this->CartesianPositionCommandRegister, 0, this->CartesianPositionCommandRegister->Length);
	Array::Copy(registers[KukaLBR4PlusWorkingRegisters::JointTorqueCommandRegister], 0, this->JointTorqueCommandRegister, 0, this->JointTorqueCommandRegister->Length);
	Array::Copy(registers[KukaLBR4PlusWorkingRegisters::TCPForceTorqueCommandRegister], 0, this->TCPForceTorqueCommandRegister, 0, this->TCPForceTorqueCommandRegister->Length);
	Array::Copy(registers[KukaLBR4PlusWorkingRegisters::JointStiffnessCommandRegister], 0, this->JointStiffnessCommandRegister, 0, this->JointStiffnessCommandRegister->Length);
	Array::Copy(registers[KukaLBR4PlusWorkingRegisters::JointDampingCommandRegister], 0, this->JointDampingCommandRegister, 0, this->JointDampingCommandRegister->Length);
	Array::Copy(registers[KukaLBR4PlusWorkingRegisters::CartesianStiffnessCommandRegister], 0, this->CartesianStiffnessCommandRegister, 0, this->CartesianStiffnessCommandRegister->Length);
	Array::Copy(registers[KukaLBR4PlusWorkingRegisters::CartesianDampingCommandRegister], 0, this->CartesianDampingCommandRegister, 0, this->CartesianDampingCommandRegister->Length);
	Array::Copy(registers[KukaLBR4PlusWorkingRegisters::CartVec1Register], 0, this->CartVec1Register, 0, this->CartVec1Register->Length);
	Array::Copy(registers[KukaLBR4PlusWorkingRegisters::CartVec2Register], 0, this->CartVec2Register, 0, this->CartVec2Register->Length);
	Array::Copy(registers[KukaLBR4PlusWorkingRegisters::CartVec3Register], 0, this->CartVec3Register, 0, this->CartVec3Register->Length);
	Array::Copy(registers[KukaLBR4PlusWorkingRegisters::JointVec1Register], 0, this->JointVec1Register, 0, this->JointVec1Register->Length);
	Array::Copy(registers[KukaLBR4PlusWorkingRegisters::JointVec2Register], 0, this->JointVec2Register, 0, this->JointVec2Register->Length);
	Array::Copy(registers[KukaLBR4PlusWorkingRegisters::JointVec3Register], 0, this->JointVec3Register, 0, this->JointVec3Register->Length);
	Array::Copy(registers[KukaLBR4PlusWorkingRegisters::Frame1Register], 0, this->Frame1Register, 0, this->Frame1Register->Length);
	Array::Copy(registers[KukaLBR4PlusWorkingRegisters::Frame2Register], 0, this->Frame2Register, 0, this->Frame2Register->Length);
	Array::Copy(registers[KukaLBR4PlusWorkingRegisters::Frame3Register], 0, this->Frame3Register, 0, this->Frame3Register->Length);
}

/// <summary> Allocate the dynamic arrays </summary>
void WorkingRegisterMessageManaged::Initialize()
{
    this->ControllerStrategyCommandRegister = 0.0;
	this->CurrentControllerStateNumber = 0.0;
    this->Scalar1Register = 0.0;
    this->Scalar2Register = 0.0;
    this->Scalar3Register = 0.0;
    this->Scalar4Register = 0.0;
    this->Scalar5Register = 0.0;
    this->Scalar6Register = 0.0;
    this->Scalar7Register = 0.0;
    this->Scalar8Register = 0.0;

    this->JointPositionCommandRegister = gcnew array<double>(NUM_JOINTS);
	this->CartesianPositionCommandRegister = gcnew array<double>(CARTESIAN_FRAME_LENGTH);
    this->JointTorqueCommandRegister = gcnew array<double>(NUM_JOINTS);
    this->TCPForceTorqueCommandRegister = gcnew array<double>(CARTESIAN_DIM);
    this->JointStiffnessCommandRegister = gcnew array<double>(NUM_JOINTS);
    this->JointDampingCommandRegister = gcnew array<double>(NUM_JOINTS);
    this->CartesianStiffnessCommandRegister = gcnew array<double>(CARTESIAN_DIM);
    this->CartesianDampingCommandRegister = gcnew array<double>(CARTESIAN_DIM);
    this->CartVec1Register = gcnew array<double>(CARTESIAN_DIM);
    this->CartVec2Register = gcnew array<double>(CARTESIAN_DIM);
    this->CartVec3Register = gcnew array<double>(CARTESIAN_DIM);
    this->JointVec1Register = gcnew array<double>(NUM_JOINTS);
    this->JointVec2Register = gcnew array<double>(NUM_JOINTS);
    this->JointVec3Register = gcnew array<double>(NUM_JOINTS);
	this->Frame1Register = gcnew array<double>(CARTESIAN_FRAME_LENGTH);
	this->Frame2Register = gcnew array<double>(CARTESIAN_FRAME_LENGTH);
	this->Frame3Register = gcnew array<double>(CARTESIAN_FRAME_LENGTH);
}
